#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S2,     gyro,                sensorI2CHiTechnicGyro)
#pragma config(Sensor, S3,     HTSMUX,              sensorI2CCustom)
#pragma config(Sensor, S4,     HTPB,                sensorLowSpeed9v)
#pragma config(Motor,  mtr_S1_C1_1,     a,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     b,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     c,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     d,             tmotorNormal, PIDControl, reversed, encoder)

#ifndef __COMMON_H__
#include "common.h"
#endif
#ifndef __HTPB_H__
#include "HTPB-driver.h"
#endif
#ifndef BIG_BOARD
#include "big_board.h"
#endif

/**********************************************************************\
*																																	  	 *
* PROGRAM: Over Bridge Driver   	      														  *
* VERSION: 1.4																										    *
* PURPOSE: This program is used to get an omni diretional robot       *
*          over the bridge, since the robot doesn't drive straight    *
* AUTHOR: Aaron Jaeger                                                *
* DATE:		 April 2011  	  																					  *
*																																		  *
* LICENSE: GNU GPL V3 2011                                            *
\**********************************************************************/


#define SAMPLES_TOTAL 4
#define THROW_AWAY 3
#define EDGE 250
#define HIGH_EDGE EDGE+
#define NOT_FALLING 0
#define FALLING_LEFT 1
#define FALLING_RIGHT 2
#define T_FIX 50
#define DRIVE_SPD 20
#define CHARGE_SPD 50
#define SLOW_SPD 35

#define IR_FL 1
#define IR_FR 2
#define IR_BL 4
#define IR_BR 8

#define FRICTION_BIAS 1.88 //1.88
#define BACK_BIAS 0.533
#define RIGHT_BIAS 0.857

void forward(int speed){
  motor[a]=speed * RIGHT_BIAS * FRICTION_BIAS   ;
  motor[b]=speed * RIGHT_BIAS * BACK_BIAS * FRICTION_BIAS ;
  motor[c]=speed * BACK_BIAS * FRICTION_BIAS ;
  motor[d]=speed;
  writeDebugStreamLine("MotorA speed: %4d", motor[a]);
  writeDebugStreamLine("MotorB speed: %4d", motor[b]);
  writeDebugStreamLine("MotorC speed: %4d", motor[c]);
  writeDebugStreamLine("MotorD speed: %4d", motor[d]);
}

void back(int speed){
  motor[a]=-speed;
  motor[b]=-speed;
  motor[c]=-speed;
  motor[d]=-speed;
}

void moveRight(int speed) {
  motor[a]=speed;
  motor[c]=speed;
  motor[b]=-speed;
  motor[d]=-speed;
}

void moveLeft(int speed) {
  motor[a]=-speed;
  motor[c]=-speed;
  motor[b]=speed;
  motor[d]=speed;
}

void moveleft_fixed(int speed){
  motor[a]=speed * RIGHT_BIAS * FRICTION_BIAS   ;
  motor[b]=-speed * RIGHT_BIAS * BACK_BIAS * FRICTION_BIAS ;
  motor[c]=speed * BACK_BIAS * FRICTION_BIAS ;
  motor[d]=-speed;
  writeDebugStreamLine("MotorA speed: %4d", motor[a]);
  writeDebugStreamLine("MotorB speed: %4d", motor[b]);
  writeDebugStreamLine("MotorC speed: %4d", motor[c]);
  writeDebugStreamLine("MotorD speed: %4d", motor[d]);
}
void fl(int speed){                        // slant forward to the left
  motor[a]=speed;
  motor[c]=speed;
  motor[b] =0;
  motor[d] =0;
}
void fr(int speed){                        // slant forward to the right
  motor[a] =0;
  motor[b]=speed;
  motor[c] =0;
  motor[d]=speed;
}

void bl (int speed){                       //Slant back to the left
  motor[a]=-speed;
  motor[b] =0;
  motor[c]=-speed;
  motor[d] =0;
}

void br (int speed) {                      //slant back to the rigt
  motor[a] =0;
  motor[b]=speed;
  motor[c] =0;
  motor[d]=speed;
}

void frontl(int speed){ // slant forward to the left
  int slower = (speed>0) ? (speed - SLOW_SPD) : (-speed - SLOW_SPD);
  motor[a]=speed;
  motor[c]=speed;
  motor[b]=slower;
  motor[d]=-slower;
}

void frontr(int speed){                        // slant forward to the right
  int slower = (speed>0) ? (speed - SLOW_SPD) : (-speed - SLOW_SPD);
  motor[a]=-slower;
  motor[b]=speed;
  motor[c]=slower;
  motor[d]=speed;
}



int getIRsensorValue(byte s) {
  byte i;
  int values[SAMPLES_TOTAL];
  int ignore[SAMPLES_TOTAL];
  for (i=0; i<SAMPLES_TOTAL; i++) {
    ignore[i]=0;
  }

  int sumVals  = 0;
  for (i=0; i<SAMPLES_TOTAL; i++) {
    values[i]=HTPBreadADC(HTPB, s, 10);
    sumVals = sumVals + values[i];
  }
  sumVals = sumVals / SAMPLES_TOTAL;

  byte j;
  for (j=0; j<THROW_AWAY; j++) {
    int worst = -1;
    byte worstIndex = 0;
    for (i=0; i<SAMPLES_TOTAL; i++) {
      if (ignore[i]==0 && abs(sumVals - values[i])>worst) {
        worst = abs(sumVals - values[i]);
        worstIndex = i;
      }
    }
    ignore[worstIndex]=1;
  }

  sumVals  = 0;
  for (i=0; i<SAMPLES_TOTAL; i++) {
    if (ignore[i]==0) {
      sumVals = sumVals + values[i];
    }
  }
  return sumVals/(SAMPLES_TOTAL-THROW_AWAY);
}

#define WIR_VAL_FL getIRsensorValue(WHEEL_IR_FL)
#define WIR_VAL_FR getIRsensorValue(WHEEL_IR_FR)
#define WIR_VAL_BL getIRsensorValue(WHEEL_IR_BL)
#define WIR_VAL_BR getIRsensorValue(WHEEL_IR_BR)

/**
*
*  FALLING_LEFT 1
*  FALLING_RIGHT 2
*  NOT_FALLING 0
*
*  1100
*  0000
*
*/
int fallingMap = 0;
int updateFallingMap() {
  fallingMap = 0;
  if (WIR_VAL_FL < EDGE) {
    fallingMap += IR_FL;
  }
  if (WIR_VAL_FR < EDGE) {
    fallingMap += IR_FR;
  }
  if (WIR_VAL_BL < EDGE) {
    fallingMap += IR_BL;
  }
  if (WIR_VAL_BR < EDGE) {
    fallingMap += IR_BR;
  }
  return fallingMap;
}




/**
* this function returns true if the robot is falling of the bridge on the left side
*/

bool FallingLeft() {
  return (fallingMap ==IR_FL)
  || (fallingMap == IR_BL)
  || (fallingMap == (IR_FL + IR_BL));
}

bool FallingRight () {
  return (fallingMap == (IR_FR + IR_BR)) || (fallingMap == IR_FR) || (fallingMap == IR_BR);
}

bool FrontOff () {
  return fallingMap == IR_FR + IR_FL;
}

bool BackOff () {
  return fallingMap == IR_BL + IR_BR;
}
bool allOn () {
  return fallingMap == 0;
}
void showValues() {
  nxtDisplayTextLine(2,"FL %d",WIR_VAL_FL);
  nxtDisplayTextLine(3,"FR %d",WIR_VAL_FR);
  nxtDisplayTextLine(4,"BL %d",WIR_VAL_BL);
  nxtDisplayTextLine(5,"BR %d",WIR_VAL_BR);

}

void debugFalling() {
  //nxtDisplayTextLine(4,"FR %4d", WIR_VAL_FL);
  if(FallingLeft() == true) {
    nxtDisplayTextLine(2,"falling left");
    } else {
    nxtDisplayTextLine(2,"------------");
  }
  if(FallingRight() == true) {
    nxtDisplayTextLine(3,"falling right");
    } else {
    nxtDisplayTextLine(3,"------------");
  }
  if(FrontOff() == true) {
    nxtDisplayTextLine(4,"FrontOff");
    } else {
    nxtDisplayTextLine(4,"------------");
  }
  if(BackOff() == true) {
    nxtDisplayTextLine(5,"BackOff");
    } else {
    nxtDisplayTextLine(5,"------------");
  }
  if(allOn() == true) {
    nxtDisplayTextLine(7,"allOn");
    } else {
    nxtDisplayTextLine(7,"------------");
  }
}



void crossBridge(bool dforward) {
  bool done = false;
  bool dirc = false;  //false -> right, true -> left

  int dspeed = (dforward) ? CHARGE_SPD : -CHARGE_SPD;
  while (!done) {
    updateFallingMap();
    //debugFalling();

    forward(dspeed);
    while(allOn() == true) {
      updateFallingMap();
      showValues();
    }
    //PlaySound(soundBeepBeep);
    if (FallingLeft()) {
      dirc = (dforward) ? true : false;
    }
    if (FallingRight()) {
      dirc = (dforward) ? false : true;
    }

    if (dirc) {
      frontr(dspeed);
      wait1Msec(T_FIX);
    }
    else {
      frontl(dspeed);
      wait1Msec(T_FIX);
    }//if dirc
  }

}

task main() {
  nMotorPIDSpeedCtrl[a] = mtrSpeedReg;
  nMotorPIDSpeedCtrl[b] = mtrSpeedReg;
  nMotorPIDSpeedCtrl[c] = mtrSpeedReg;
  nMotorPIDSpeedCtrl[d] = mtrSpeedReg;

  while(true) {}
  while(true){
    crossBridge(true);
  }
}
